#!/usr/bin/env python3
"""
Dynamic Heading Descent Control with Logging
----------------------------------------------
该文件实现飞机实时位置与下滑道路径比较，并将差异保存到cy.csv 文件中。
主要改进：
1. 动态计算距跑道15公里处的下滑道截获点(GSIP)
2. 实时生成下滑道路径
3. 增强数据记录功能
4. 新增下滑道参数导出功能
"""

import math
import time
import logging
import csv
from geopy.distance import geodesic
from geopy.point import Point
from typing import Dict, List
import matplotlib.pyplot as plt
from getDCSdata import get_dcs_data
from keyboard_control import (
    activate_dcs_window,
    pitch_up, pitch_down,
    throttle_up, throttle_down,
    roll_left, roll_right
)

# ==============================
# 基本参数设置
# ==============================
RUNWAY_LAT = 41.929631
RUNWAY_LONG = 41.86325
RUNWAY_ALT = 18.0  # 跑道海拔高度（米）
APPROACH_SPEED = 70.0  # 目标进近速度（m/s）
RUNWAY_HEADING = 70.0  # 跑道方向（度）
GLIDE_ANGLE = math.radians(3.0)  # 下滑道角度
ALTITUDE_TOLERANCE = 5.0  # 高度容忍度
CONTROL_INTERVAL = 0.3  # 控制周期（秒）
MAX_HEADING_ERROR = 5.0  # 航向误差容忍度（度）
INTERCEPT_DISTANCE = 15000  # 截获点距离跑道入口的距离（米）

# ==============================
# 日志配置
# ==============================
logging.basicConfig(
    level=logging.INFO,
    format='%(asctime)s - %(levelname)s - %(message)s',
    handlers=[
        logging.FileHandler('descent_control.log'),
        logging.StreamHandler()
    ]
)
logger = logging.getLogger(__name__)


class DynamicGlidePathGenerator:
    def __init__(self):
        self.runway_point = Point(RUNWAY_LAT, RUNWAY_LONG)
        self.current_gsip = None
        self.glide_path = []

    def calculate_gsip(self) -> Point:
        """根据跑道航向和固定距离计算下滑道截获点"""
        gsip = geodesic(kilometers=INTERCEPT_DISTANCE / 1000).destination(
            self.runway_point, RUNWAY_HEADING+180)
        gsip_alt = RUNWAY_ALT + INTERCEPT_DISTANCE * math.tan(GLIDE_ANGLE)
        self.current_gsip = Point(gsip.latitude, gsip.longitude, gsip_alt)
        logger.info(f" 计算GSIP: 纬度={gsip.latitude:.6f},  经度={gsip.longitude:.6f},  高度={gsip_alt:.1f}m")
        return self.current_gsip

    def generate_glide_path(self, step: int = 100) -> List[Dict[str, float]]:
        """动态生成下滑道路径"""
        if not self.current_gsip:
            self.calculate_gsip()

        total_dist = INTERCEPT_DISTANCE
        path = []

        for dist in range(0, int(total_dist) + step, step):
            point = geodesic(kilometers=dist / 1000).destination(
                Point(self.current_gsip.latitude, self.current_gsip.longitude),
                (RUNWAY_HEADING + 180) % 360  # 反向计算
            )
            alt = RUNWAY_ALT + (total_dist - dist) * math.tan(GLIDE_ANGLE)
            path.append({
                "lat": point.latitude,
                "lon": point.longitude,
                "alt": alt,
                "dist_from_threshold": total_dist - dist
            })

        self.glide_path = path
        return path

    def export_glide_path(self, filename: str = "glide_path_params.csv"):
        """
        导出下滑道参数到CSV文件
        格式: 序号,纬度,经度,海拔,距跑道距离
        """
        if not self.glide_path:
            self.generate_glide_path()

        with open(filename, 'w', newline='') as csvfile:
            writer = csv.writer(csvfile)
            writer.writerow([' 序号', '纬度', '经度', '海拔(m)', '距跑道距离(m)'])

            for idx, point in enumerate(self.glide_path):
                writer.writerow([
                    idx + 1,
                    f"{point['lat']:.6f}",
                    f"{point['lon']:.6f}",
                    f"{point['alt']:.1f}",
                    point['dist_from_threshold']
                ])

        logger.info(f" 下滑道参数已导出到{filename}")

    def plot_glide_path(self):
        """绘制下滑道剖面图（调试用）"""
        if not self.glide_path:
            return

        dist = [p['dist_from_threshold'] / 1000 for p in self.glide_path]
        alt = [p['alt'] for p in self.glide_path]
        plt.figure(figsize=(10, 5))
        plt.plot(dist, alt, 'b-', label='下滑道剖面')
        plt.xlabel(' 距跑道距离 (km)')
        plt.ylabel(' 高度 (m)')
        plt.title(f' 下滑道剖面 (角度={math.degrees(GLIDE_ANGLE):.1f}°)')
        plt.grid(True)
        plt.legend()
        plt.savefig('glide_path_profile.png')
        plt.close()


class DescentController:
    def __init__(self, approach_speed: float):
        self.generator = DynamicGlidePathGenerator()
        self.glide_path = self.generator.generate_glide_path()
        self.approach_speed = approach_speed
        self.in_descent_mode = False
        self.last_control_time = time.time()
        self.last_path_update = time.time()

        # 初始化CSV文件
        with open('cy.csv', 'w', newline='') as csvfile:
            fieldnames = ['timestamp', 'real_lat', 'real_lon', 'real_alt',
                          'nearest_lat', 'nearest_lon', 'nearest_alt',
                          'alt_diff', 'lat_diff', 'lon_diff',
                          'gsip_lat', 'gsip_lon', 'gsip_alt',
                          'dist_to_threshold']
            writer = csv.DictWriter(csvfile, fieldnames=fieldnames)
            writer.writeheader()

    def run(self):
        """控制主循环"""
        try:
            # 初始绘制下滑道剖面
            self.generator.plot_glide_path()
            # 导出下滑道参数
            self.generator.export_glide_path()

            while not self._should_terminate():
                self._control_cycle()
        except KeyboardInterrupt:
            logger.info("Control  stopped by user")

    def _control_cycle(self):
        """单次控制周期"""
        data = self._get_valid_data()
        if not data:
            return

            # 每10秒更新一次下滑道
        if time.time() - self.last_path_update > 10.0:
            self.glide_path = self.generator.generate_glide_path()
            self.last_path_update = time.time()
            logger.debug(" 更新下滑道路径")

        current_pos = Point(data["LLA_lat"], data["LLA_long"])
        current_alt = data["LLA_balt"]
        current_heading = data["heading_deg"]

        # 比较飞机位置与下滑道路径
        nearest_point = self._get_nearest_point(current_pos)
        self._log_position_difference(current_pos, current_alt, nearest_point)

        # 确定是否进入下降模式
        if not self.in_descent_mode:
            distance_to_intercept = geodesic(
                (current_pos.latitude, current_pos.longitude),
                (self.glide_path[0]["lat"], self.glide_path[0]["lon"])
            ).meters
            if distance_to_intercept < 2000 and abs(current_heading - RUNWAY_HEADING) < MAX_HEADING_ERROR:
                self.in_descent_mode = True
                logger.info("DESCENT  MODE ACTIVATED")

        if self.in_descent_mode:
            self._control_glide_slope(current_alt, current_pos, current_heading)
        else:
            self._control_navigation(current_pos, current_heading)

        self._throttle_control(data["IAS"])
        self._sleep_until_next_cycle()

    def _log_position_difference(self, current_pos: Point, current_alt: float,
                                 nearest_point: Dict[str, float]):
        """记录数据时包含GSIP信息"""
        alt_diff = current_alt - nearest_point["alt"]
        lat_diff = current_pos.latitude - nearest_point["lat"]
        lon_diff = current_pos.longitude - nearest_point["lon"]
        dist_to_threshold = nearest_point.get("dist_from_threshold", 0)

        with open('cy.csv', 'a', newline='') as csvfile:
            writer = csv.writer(csvfile)
            writer.writerow([
                time.strftime("%Y-%m-%d  %H:%M:%S"),
                current_pos.latitude, current_pos.longitude, current_alt,
                nearest_point["lat"], nearest_point["lon"], nearest_point["alt"],
                alt_diff, lat_diff, lon_diff,
                self.generator.current_gsip.latitude,
                self.generator.current_gsip.longitude,
                self.generator.current_gsip.altitude,
                dist_to_threshold
            ])

    def _control_navigation(self, current_pos: Point, current_heading: float):
        """导航到截获区域"""
        target_heading = math.degrees(math.atan2(
            self.glide_path[0]["lon"] - current_pos.longitude,
            self.glide_path[0]["lat"] - current_pos.latitude
        ))
        heading_error = (target_heading - current_heading + 180) % 360 - 180

        if heading_error > MAX_HEADING_ERROR:
            roll_right(duration=0.3)
        elif heading_error < -MAX_HEADING_ERROR:
            roll_left(duration=0.3)
        logger.debug(f"Heading  corrected, error: {heading_error:.1f}")

    def _control_glide_slope(self, current_alt: float, current_pos: Point, current_heading: float):
        """跟随下滑道路径飞行"""
        nearest_point = self._get_nearest_point(current_pos)
        target_alt = nearest_point["alt"]
        alt_error = current_alt - target_alt

        if alt_error > ALTITUDE_TOLERANCE:
            pitch_down(duration=min(1.0, alt_error / 50))
        elif alt_error < -ALTITUDE_TOLERANCE:
            pitch_up(duration=min(1.0, abs(alt_error) / 50))
        logger.debug(f"Altitude  corrected, error: {alt_error:.1f}m")

        # 航向修正（沿路径方向）
        target_heading = math.degrees(math.atan2(
            # nearest_point["lon"] - current_pos.longitude,
            nearest_point["lat"] - current_pos.latitude,
            nearest_point["lon"] - current_pos.longitude
        ))
        heading_error = (target_heading - current_heading +180) % 360 - 180

        if heading_error > MAX_HEADING_ERROR:
            roll_right(duration=0.2)
        elif heading_error < -MAX_HEADING_ERROR:
            roll_left(duration=0.2)
        logger.debug(f"Path  heading corrected, error: {heading_error:.1f}")

    def _get_nearest_point(self, current_pos: Point) -> Dict[str, float]:
        """获取下滑道上最近的点"""
        return min(
            self.glide_path,
            key=lambda point: geodesic((current_pos.latitude, current_pos.longitude),
                                       (point["lat"], point["lon"])).meters
        )

    def _throttle_control(self, current_speed: float):
        """速度控制"""
        speed_error = current_speed - self.approach_speed
        if speed_error > 5:
            throttle_down(duration=0.3)
        elif speed_error < -5:
            throttle_up(duration=0.3)
        logger.debug(f"Throttle  adjusted, error: {speed_error:.1f}m/s")

    def _should_terminate(self) -> bool:
        """检查是否到达跑道"""
        # 简单实现：当高度接近跑道高度时终止
        data = self._get_valid_data()
        if data and (data["LLA_balt"] - RUNWAY_ALT) < 10:
            logger.info("Approach  completed: reached runway altitude")
            return True
        return False

    def _get_valid_data(self) -> Dict:
        """获取飞机数据并处理异常"""
        try:
            activate_dcs_window()
            return get_dcs_data()
        except Exception as e:
            logger.warning(f"Data  retrieval failed: {e}")
            return None

    def _sleep_until_next_cycle(self):
        """控制周期休眠"""
        elapsed = time.time() - self.last_control_time
        sleep_time = max(0.01, CONTROL_INTERVAL - elapsed)
        time.sleep(sleep_time)
        self.last_control_time = time.time()


def main():
    """主函数入口"""
    # 初始化下降控制器
    controller = DescentController(approach_speed=APPROACH_SPEED)

    # 打印初始下滑道信息
    gsip = controller.generator.current_gsip
    logger.info(f" 初始下滑道截获点(GSIP): "
                f"纬度={gsip.latitude:.6f},  经度={gsip.longitude:.6f},  高度={gsip.altitude:.1f}m")
    logger.info(f" 下滑道角度: {math.degrees(GLIDE_ANGLE):.1f}°")

    # 运行控制逻辑
    controller.run()


if __name__ == "__main__":
    main()